/*******************************************************************************
* This file implements the Direction Cosine Matrix (DCM) to calculate the 
* attitude base on the data from the IMU.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#ifndef _DCM_H
#define _DCM_H



#include <math.h>
#include "system.h"
#include "variables.h"
#include "task.h"
#include "hal_sensors.h"
#include "hal_gps.h"
#include "hal_ic.h"



/*******************************************************************************
* STRUCTURE AND UNION DEFINITION                                               *
*******************************************************************************/

// Attitude of the aircraft.
// Unit = radian.
struct ATTITUDE{
	float fRoll;		// Range: -PI to PI.
	float fPitch;		// Range: -0.5PI to 0.5PI.
	float fYaw;		// Range: 0 to 2PI.
	float fMagHeading;	// Range: 0 to 2PI.
	
	float fRateX;		// Unit: rad/s.
	float fRateY;		// Unit: rad/s.
	float fRateZ;		// Unit: rad/s.
	
	float fRateBiasX;	// Unit: rad/s.
	float fRateBiasY;	// Unit: rad/s.
	float fRateBiasZ;	// Unit: rad/s.
};



/*******************************************************************************
* PUBLIC FUNCTION PROTOTYPES                                                   *
*******************************************************************************/

/*******************************************************************************
* PUBLIC FUNCTION: fGetRollAcc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The roll angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated roll angle from accelerometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetRollAcc(void);



/*******************************************************************************
* PUBLIC FUNCTION: fGetPitchAcc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The pitch angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated pitch angle from accelerometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetPitchAcc(void);



/*******************************************************************************
* PUBLIC FUNCTION: fGetYawMag
*
* PARAMETERS:
* ~ fPitch  - Pitch angle in rad/s.
* ~ fRoll   - Roll angle in rad/s.
*
* RETURN:
* ~ The yaw angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the uncompensated yaw angle from magnetometer.
* Need to update the sensors reading before calling this function.
*
*******************************************************************************/
float fGetYawMag(float fPitch, float fRoll);



/*******************************************************************************
* PUBLIC FUNCTION: xGetAttitude
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The pitch, roll and yaw angle.
* ~ Unit = radian.
*
* DESCRIPTIONS:
* Get the pitch, roll and yaw angle of the system.
*
*******************************************************************************/
struct ATTITUDE xGetAttitude(void);



/*******************************************************************************
* PUBLIC FUNCTION: fGetDcm22
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The value in DCM[2][2].
*
* DESCRIPTIONS:
* Get the value of DCM[2][2].
*
*******************************************************************************/
float fGetDcm22(void);



/*******************************************************************************
* TASK: taskDcmLoop
*
* DESCRIPTIONS:
* This task is the main loop of the DCM and will get executed every 5ms (200Hz).
*
*******************************************************************************/
portTASK_FUNCTION_PROTO(taskDcmLoop, pvParameters);



#endif
